Control Theory¶

This notebook explore the dynamics and control of the cruise control system of a vehicle.

Cruise Control System¶

The Cruise Control is a system that automatically controls the speed of a motor vehicle. Applying some simplifications, we can describe the systems with the following block diagram.

Block Diagram

Where the User inputs a desired Velocity [km/h]. This value is translated into bits [b] so it can be read by the Engine Control Unit (ECU). Where a compensator signals the Electric Throttle Controller (ETC) to determine a percentage [0-100%]. This signal is also in bits, so there should be a component that converts the signal into voltage [V] to control the Motor, this motor produces a driving force that impulses the vehicle forwards.

Now, there are external forces that act against the vehicle's motion. We call these forces Disturbances. The sum of these forces is the disturbance force, $F_{d}$. We can make a process out of Newton's second law, $F=ma$ to explain the Vehicle's dynamics. Where the final velocity can be obtained from the integral of the acceleration.

The feedback signal results from a Speedometer readings (which can also be affected), there should be also a conversor so the signal error can be generated from the difference between the desired and the actual velocity.

Dynamics¶

While this does not intend to be a comprehensive vehicle dynamics model, we can use a simplified model to simulate the behavior of a vehicle. The vehicle produces a force that propels it forward. This force, $F$, represents the vehicle's engine generated force.

We will define this force as:

$$F = \alpha \ T_{m} \ [1 - \beta \ (\frac{\omega}{\omega_{m}} - 1)^2] \ t$$

where:

  • $\alpha$ is the gear ratio / wheel radius [1/m]
  • $T_{m}$ is the maximum torque [Nm]
  • $\beta$ is the peak engine rolloff [dimensionless]
  • $\omega$ is the angular velocity [rad/s]
  • $\omega_{m}$ is the vehicle's maximum angular velocity [rad/s]
  • $t$ is the throttle input, a value between $0$ and $1$ [dimensionless]

Those values can be found in the vehicle's specifications.

This law also states that the acceleration of an object is directly proportional to the net force acting on it and inversely proportional to its mass. The law is given by the equation:

$$F = m \ a$$

For our vehicle model, the net force is the sum of the throttle force and the disturbance forces:

$$F_{net} = F - F_{d}$$

Then, the acceleration is given by:

$$a = \frac{F_{net}}{m}$$

Disturbance Forces¶

We will take into account three major forces that act against the vehicle's motion. The sum of these forces will the disturbance force, $F_{d}$.

$$F_{d} = F_{g} + F_{r} + F_{a}$$

Gravity¶

The force of gravity is given by:

$$F_{g} = m \ g \ sin(\theta)$$

where:

  • $m$ is the mass of the vehicle
  • $g$ is the acceleration due to gravity
  • $\theta$ is the elevation angle of the road

Rolling Friction¶

Rolling friction is the force that opposes the motion of the vehicle. It is given by:

$$F_{r} = \mu \ N$$

where:

  • $\mu$ is the coefficient of rolling friction
  • $N$ is the normal force

Aerodynamic Drag Force¶

Drag depends on the properties of the fluid and on the size, shape, and speed of the object. One way to express this is by means of the drag equation:

$$F_{a} = \frac{1}{2} \ c_{d} \ A \ \rho \ v^2$$

where:

  • $F_{d}$ is the drag force
  • $\rho$ is the density of the fluid
  • $v$ is the speed of the object relative to the fluid
  • $A$ is the area of the object facing the fluid
  • $c_{d}$ is the drag coefficient - a dimensionless number

As we will evaluate aerodynamics, the 'fluid' is the air.

At $101.325\ kPa$ (abs) and $15 °C$ ($59 °F$), air has a density of approximately $1.225 kg/m^3$ according to the International Standard Atmosphere (ISA).

Typical Drag Coefficients values¶
Object Drag Coefficient ($c_{d}$)
Airfoil 0.05
Toyota Camry 0.28
Ford Focus 0.32
Honda Civic 0.36
Ferrari Testarossa 0.37
Dodge Ram pickup 0.43
Bicycle 0.9

Source: College Physics, OpenStax

InĀ [1]:
from math import copysign, sin

import numpy as np
import pandas as pd
import seaborn as sns
from matplotlib import pyplot as plt
from pydantic import BaseModel

# Constants
air_density = 1.225  # kg/m^3
g = 9.80665  # m/s^2

Vehicle Model¶

We can model a simplified vehicle dynamics using a python class.

InĀ [2]:
class Vehicle(BaseModel):
    """
    Vehicle Dynamics Model
    
    Attributes:
        position: current position of the vehicle (m)
        speed: current speed of the vehicle (m/s)
        mass: mass of the vehicle (Kg)
        max_throttle: maximum throttle force (N)
        drag_coefficient: drag coefficient (dimensionless)
        frontal_area: frontal area of the vehicle (m^2)
    """
    position: float = 0  # m
    speed: float = 0  # m/s
    mass: float  # Kg
    max_throttle: float  # N
    drag_coefficient: float  # dimensionless
    frontal_area: float  # m^2

Toyota Camry Example¶

Toyota Camry

Toyota Camry XSE 2025 (4T1DAACK2SU017407)

Specs:

2.5-Liter, 4-cylinder DOHC 16-valve, Variable Valve Timing-intelligence (VVT-i) with Variable Valve Timing-intelligence by Electric motor(VVT-iE) on the intake side, 183 hp @ 6000 rpm, 163 lb.-ft. @ 3600-5200 rpm: 2.5-Liter, 4-cylinder DOHC 16-valve, Variable Valve Timing-intelligence (VVT-i) with Variable Valve Timing-intelligence by Electric motor(VVT-iE) on the intake side, 183 hp @ 6000 rpm, 163 lb.-ft. @ 3600-5200 rpm

According to the Toyota specifications, this unit drag coefficient ($c_{d}$) is approximately $0.37$ and the frontal area ($A$) is $1.94 m^2$. It weighs $3538 \ lbs$ ($1604 \ kg$) and has a maximum horsepower of $184hp$ ($137209 \ W$), and maximum top speed of $130mph$ ($209.2km/h$ or $58.33 m/s$). It's maximum torque $T_{m}$ is $163 \ lb.-ft.$ ($221 \ Nm$), and peak engine angular velocity $\omega_{m}$ is $5200 \ rpm$ ($545.3 \ rad/s$).

InĀ [3]:
camry = Vehicle(mass=1604, max_throttle=2352, drag_coefficient=0.37, frontal_area=1.94)
print(f"Toyota Camry XSE 2025({camry})")
Toyota Camry XSE 2025(position=0 speed=0 mass=1604.0 max_throttle=2352.0 drag_coefficient=0.37 frontal_area=1.94)

For our process or plant, we will use a simple model that simulates the vehicle dynamics. This model will have a method that updates the vehicle's position and speed based on the throttle input and the time step.

InĀ [4]:
from math import radians

# Sign function
sign = lambda x: copysign(1, x)


def process(vehicle: Vehicle, throttle: float, dt: float, theta: float = 0.0, mu: float = 0.01) -> float:
    """
    Simulate vehicle dynamics updating its position and speed.
    
    Args:
        vehicle: a dynamic model
        throttle: percentage of throttle input
        dt: time step
        theta: inclination angle of the road
        mu: coefficient of rolling friction

    Returns:
        vehicle speed
    """
    m = vehicle.mass  # Kg
    v = vehicle.speed  # m/s
    area = vehicle.frontal_area  # m^2

    # Force generated by the throttle input
    f = throttle * vehicle.max_throttle

    # force Fg = m g sin \theta.
    fg = m * g * sin(radians(theta))

    # A simple model of rolling friction is Fr = m g Cr sgn(v), where Cr is
    # the coefficient of rolling friction and sgn(v) is the sign of v (±1) or
    # zero if v = 0.
    fr = m * g * mu * sign(v)

    # Fa = 1/2 * Cd * A * rho * v^2
    fa = 0.5 * vehicle.drag_coefficient * area * air_density * v ** 2

    # Total disturbance force
    fd = fg + fr + fa

    # a = F/m
    a = (f - fd) / m

    # Update vehicle position and speed
    vehicle.position += v * dt
    vehicle.speed += a * dt

    if vehicle.speed < 0:
        vehicle.speed = 0

    return vehicle.speed

We have built now a simple open-loop model of the vehicle dynamics.

Open Loop

InĀ [5]:
class Simulation(BaseModel):
    times: list[float] = []
    speeds: list[float] = []
    positions: list[float] = []
    errors: list[float] = []
    inclinations: list[float] = []


def simulate(vehicle: Vehicle, total_time: float = 3_600.0, dt: float = 0.1) -> Simulation:
    sim = Simulation()

    # Reset vehicle state
    vehicle.position = 0
    vehicle.speed = 0

    for t in np.arange(0, total_time, dt):
        throttle = 0.75  # %
        speed = process(vehicle, throttle, dt)

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)

    return sim
InĀ [6]:
def plot_results(simulation: Simulation, set_speed: float = 0.0):
    plt.clf()
    plt.close()
    plt.figure(figsize=(12, 6))
    plt.subplot(3, 1, 1)
    plt.plot(simulation.times, simulation.speeds, label='Speed')
    if set_speed > 0.0:
        plt.axhline(y=set_speed, color='r', linestyle='--', label='Set Speed')
    plt.xlabel('Time (s)')
    plt.ylabel('Speed (m/s)')
    plt.title('Vehicle Speed Over Time')
    plt.legend()

    plt.subplot(3, 1, 2)
    plt.plot(simulation.times, simulation.positions, label='Position')
    plt.xlabel('Time (s)')
    plt.ylabel('Position (m)')
    plt.title('Vehicle Position Over Time')
    plt.legend()

    if len(simulation.errors) > 0:
        plt.subplot(3, 1, 3)
        plt.plot(simulation.times, simulation.errors, label='Error')
        plt.xlabel('Time (s)')
        plt.ylabel('Error (m/s)')
        plt.title('Speed Error Over Time')
        plt.legend()

    plt.tight_layout()
    plt.show()


open_loop = simulate(camry)
plot_results(open_loop)
No description has been provided for this image

Nothing surprising here, as no control is applied, the vehicle accelerates until it reaches a maximum speed according to the throttle input and the disturbance forces acting on it.

Let's now implement a simple closed-loop system to control the speed of the vehicle.

Closed Loop

Where the controller:

PID

InĀ [7]:
class PIDController(BaseModel):
    """
    Proportional-Integral-Derivative Controller
    
    Attributes:
        kp: proportional gain
        ki: integral gain
        kd: derivative gain
    """
    kp: float
    ki: float = 0.0
    kd: float = 0.0

    _integral: float = 0
    _previous_error: float = 0

    def update(self, error: float, dt: float) -> float:
        """
        Update PID controller output
        
        Args:
            error: signal error
            dt: time step
        
        Returns:
            A value between [-1, 1] control signal
        """
        self._integral += error * dt

        derivative = (error - self._previous_error) / dt

        # u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt
        output = self.kp * error + self.ki * self._integral + self.kd * derivative
        self._previous_error = error

        return np.clip(output, -1, 1)

Update the simulation function to include the controller.

InĀ [8]:
from typing import Callable


def simulate(vehicle: Vehicle,
             set_speed: float,
             controller: Callable[[float, float], float],
             total_time: float = 3_600.0,
             dt: float = 0.1) -> Simulation:
    sim = Simulation()

    vehicle.speed = 0
    vehicle.position = 0

    for t in np.arange(0, total_time, dt):
        error = set_speed - vehicle.speed
        throttle = controller(error, dt)  # %
        speed = process(vehicle, throttle, dt)

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)
        sim.errors.append(error)

    return sim

Comparing Controllers¶

We will compare the performance of the controllers by setting a desired speed of $30 \ m/s$ (108 km/h) and evaluating the vehicle's response.

InĀ [9]:
def calculate_settling_time(times: list, output: list, percentage: float = 0.05):
    """
    Calculate the settling time from the system output.

    Parameters:
    times (list): Recorded time of the system.
    output (list): System output.
    percentage (float): Tolerance percentage, by default 5%.

    Returns:
    float: settling time
    """
    final_value = output[-1]
    # bounds for 5% tolerance
    lower_bound = final_value * (1 - percentage)
    upper_bound = final_value * (1 + percentage)

    # find the first index where the output is within the bounds
    for i in reversed(range(len(output))):
        if not lower_bound <= output[i] <= upper_bound:
            # the last time the output was outside bounds + one step (we left the bounds)
            return times[i + 1]

    # output was always within the bounds
    return times[0]
InĀ [10]:
p = PIDController(kp=1)
desired_speed = 30  # m/s (108 km/h)
p_simulation = simulate(camry, set_speed=desired_speed, controller=p.update)
plot_results(p_simulation, set_speed=desired_speed)
No description has been provided for this image
InĀ [11]:
pi = PIDController(kp=1, ki=0.5)
pi_simulation = simulate(camry, set_speed=desired_speed, controller=pi.update)
plot_results(pi_simulation, set_speed=desired_speed)
No description has been provided for this image
InĀ [12]:
pid = PIDController(kp=1, ki=0.5, kd=2)
pid_simulation = simulate(camry, set_speed=desired_speed, controller=pid.update)
plot_results(pid_simulation, set_speed=desired_speed)
No description has been provided for this image
InĀ [13]:
t_p = calculate_settling_time(p_simulation.times, p_simulation.speeds)
t_pi = calculate_settling_time(pi_simulation.times, pi_simulation.speeds)
t_pid = calculate_settling_time(pid_simulation.times, pid_simulation.speeds)
print(f"Settling Time (P): {t_p:.2f} s")
print(f"Settling Time (PI): {t_pi:.2f} s")
print(f"Settling Time (PID): {t_pid:.2f} s")
Settling Time (P): 21.80 s
Settling Time (PI): 86.00 s
Settling Time (PID): 97.00 s

Disturbance¶

Let's add a disturbance to the system and evaluate the controllers' performance.

We will simulate a sudden change in the road inclination angle.

InĀ [14]:
from scipy.stats import semicircular


def produce_inclination(prev_theta: float = 0.0,
                        min_theta: float = -5.0,
                        max_theta: float = 20.0,
                        ) -> float:
    # Random change in the inclination angle
    change = semicircular.rvs()

    # Update the inclination angle limiting its value
    theta = round(prev_theta + change, 2)

    return np.clip(theta, min_theta, max_theta)
InĀ [15]:
current_theta = 0.0
inclinations = []
for _ in range(500):
    current_theta = produce_inclination(prev_theta=current_theta)
    inclinations.append(current_theta)

# Plotting
sns.histplot(inclinations, bins=400, kde=True)
plt.xlabel('Inclination')
plt.ylabel('Frequency')
plt.title('Histogram of Generated Road Inclinations')
plt.show()
No description has been provided for this image

Now, these inclinations change do not happen at regular intervals, so we will update the simulation function to include the disturbance.

We will use Maxwell distribution to generate random intervals between disturbances.

$$f(x) = \sqrt{2/\pi} x^2 e^{-x^2/2}$$

InĀ [16]:
from scipy.stats import maxwell

fig, ax = plt.subplots(1, 1)
rv = maxwell()

x = np.linspace(maxwell.ppf(0.01),
                maxwell.ppf(0.99), 100)
ax.plot(x, maxwell.pdf(x), 'r-', lw=5, alpha=0.6, label='maxwell pdf')
ax.set_xlim([x[0], x[-1]])
plt.show()
No description has been provided for this image
InĀ [17]:
def interval_generator() -> list[int]:
    """
    Generate random intervals between disturbances.
    """
    intervals = maxwell.rvs(loc=5, scale=750, size=3_600)
    return list(set(map(int, sorted(intervals))))

Update the simulation function to include the disturbance.

InĀ [18]:
def adjust_inclination(theta: float = 0.0,
                       rate: float = 0.5,
                       ) -> float:
    if theta >= 0:
        return np.clip(theta - rate, 0, theta)
    elif theta < 0:
        return np.clip(theta + rate, theta, 0)


def generate_disturbances() -> dict:
    """
    Generate random disturbances.
    """
    times = interval_generator()
    return {t: produce_inclination() for t in times}


road_inclinations = generate_disturbances()


def simulate_with_disturbance(vehicle: Vehicle,
                              set_speed: float,
                              controller: Callable[[float, float], float],
                              total_time: float = 3_600.0,
                              dt: float = 1.0) -> Simulation:
    sim = Simulation()

    # Set Initial State
    vehicle.speed = 0.0
    vehicle.position = 0.0
    inclination_angle = 0.0
    t_last_disturbance = 0

    for t in np.arange(0, total_time, dt):

        # Check for disturbances
        if int(t) in road_inclinations.keys():
            inclination_angle = road_inclinations[int(t)]
            t_last_disturbance = t
        # Gradual change in the inclination angle after a disturbance to simulate the road leveling
        if t_last_disturbance + 2 > t:
            inclination_angle = adjust_inclination(inclination_angle)

        error = set_speed - vehicle.speed
        throttle = controller(error, dt)  # %
        speed = process(vehicle, throttle, dt, theta=inclination_angle)

        if speed > 70.0:
            print(
                f"SPEED Error at {t}s, speed: {speed} m/s, throttle: {throttle * 100} %, error: {error} m/s, inclination: {inclination_angle}")
            break

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)
        sim.errors.append(error)

    return sim
InĀ [19]:
p_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=p.update, total_time=45 * 60)
plot_results(p_simulation, set_speed=desired_speed)
No description has been provided for this image
InĀ [20]:
pi_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=pi.update, total_time=45 * 60)
plot_results(pi_simulation, set_speed=desired_speed)
No description has been provided for this image
InĀ [21]:
pid_simulation = simulate_with_disturbance(camry, set_speed=desired_speed, controller=pid.update, total_time=45 * 60)
plot_results(pid_simulation, set_speed=desired_speed)
No description has been provided for this image
InĀ [22]:
data = {
    "Simulation": ["P", "PI", "PID"],
    "Max Distance (km)": [max(p_simulation.positions) / 1_000, max(pi_simulation.positions) / 1_000,
                          max(pid_simulation.positions) / 1_000],
    "Average Speed": [sum(p_simulation.speeds) / len(p_simulation.speeds),
                      sum(pi_simulation.speeds) / len(pi_simulation.speeds),
                      sum(pid_simulation.speeds) / len(pid_simulation.speeds)],
    "Max Speed": [max(p_simulation.speeds), max(pi_simulation.speeds), max(pid_simulation.speeds)],
    "Min Speed": [min(p_simulation.speeds), min(pi_simulation.speeds), min(pid_simulation.speeds)],
    "Max Error": [max(p_simulation.errors), max(pi_simulation.errors), max(pid_simulation.errors)],
    "Min Error": [min(p_simulation.errors), min(pi_simulation.errors), min(pid_simulation.errors)]
}

pd.DataFrame(data)
Out[22]:
Simulation Max Distance (km) Average Speed Max Speed Min Speed Max Error Min Error
0 P 80.022734 29.649075 29.866562 1.368268 30.0 0.133438
1 PI 81.000000 30.011111 52.480769 1.368268 30.0 -22.480769
2 PID 80.999246 30.010498 52.480769 1.368268 30.0 -22.480769

Speedometer¶

The speedometer is a device that measures the instantaneous speed of a vehicle. While manufacturers have different technologies to measure speed, and calibrate their speedometers according to the vehicle's specifications, we can simulate a simple speedometer error as a percentage of the actual speed.

Let's simulate that the speedometer may have an error $+/- 2 km/h$, Which could translate to $0.5556 m/s$. For this simulation, we will consider a normal distribution for errors, and uniform random noise to simulate the speedometer's readings.

Also, Speedometers have a range of readings, so we will limit the readings between $0$ and $75 m/s$.

InĀ [23]:
SPEEDOMETER_MIN = 0  # m/s
SPEEDOMETER_MAX = 75  # m/s


def measure_element(speed: float) -> float:
    """
    Measure the element with a 98% accuracy.
    """
    error = np.random.normal(0.2778, 0.5556)
    random = np.random.choice([1, -1])
    return np.clip(speed + random * error, SPEEDOMETER_MIN, SPEEDOMETER_MAX)

Let's see some examples of speedometer readings.

InĀ [24]:
sample_measured_speeds = [measure_element(speed) for speed in p_simulation.speeds]

plt.figure(figsize=(12, 6))
plt.plot(p_simulation.times, p_simulation.speeds, label='Actual Speed')
plt.plot(p_simulation.times, sample_measured_speeds, label='Measured Speed')
plt.xlabel('Time (s)')
plt.ylabel('Speed (m/s)')
plt.title('Actual vs Measured Speed')
plt.legend()
plt.show()
No description has been provided for this image
InĀ [25]:
speed_errors = [actual - measured for actual, measured in zip(p_simulation.speeds, sample_measured_speeds)]
plt.figure(figsize=(10, 6))
plt.plot(p_simulation.times, speed_errors)
plt.xlabel('Time (s)')
plt.ylabel('Speed Error (units as per speed)')
plt.title('Error in Speed Over Time')
plt.show()
No description has been provided for this image
InĀ [26]:
plt.figure(figsize=(10, 6))
plt.hist(speed_errors, bins=30)
plt.xlabel('Speed Error (units as per speed)')
plt.ylabel('Frequency')
plt.title('Distribution of Speed Errors')
plt.show()
No description has been provided for this image
InĀ [27]:
print(
    f"Percentage of less readings than actual speed: {len([e for e in speed_errors if e < 0]) / len(speed_errors) * 100:.2f}%")
print(
    f"Percentage of more readings than actual speed: {len([e for e in speed_errors if e > 0]) / len(speed_errors) * 100:.2f}%")
Percentage of less readings than actual speed: 49.89%
Percentage of more readings than actual speed: 50.11%
InĀ [28]:
df_speedometer = pd.DataFrame({
    "Actual Speed": p_simulation.speeds,
    "Measured Speed": sample_measured_speeds,
    "Speed Error": speed_errors
})
df_speedometer.describe()
Out[28]:
Actual Speed Measured Speed Speed Error
count 2700.000000 2700.000000 2700.000000
mean 29.649075 29.649936 -0.000862
std 1.498468 1.620539 0.611654
min 1.368268 1.753615 -2.265078
25% 29.763037 29.354752 -0.392556
50% 29.767485 29.760115 0.002619
75% 29.771409 30.154742 0.402372
max 29.866562 31.970968 1.762274
InĀ [29]:
df_speedometer.head()
Out[29]:
Actual Speed Measured Speed Speed Error
0 1.368268 2.524395 -1.156127
1 2.736022 1.753615 0.982407
2 4.102238 4.332648 -0.230410
3 5.465893 5.144739 0.321154
4 6.825972 6.327222 0.498750

Having the speedometer readings in mind, we can now say that our vehicle is in a stable state when the error is within the $4 km/h$ tolerance of the actual speed.

InĀ [30]:
def ms_to_kmh(speed: float) -> float:
    """
    Speed conversion
    
    Args:
        speed: speed in m/s
    
    Returns:
        speed in km/h
    """
    return speed * 3.6
InĀ [31]:
def simulate_with_readings(vehicle: Vehicle,
                           set_speed: float,
                           controller: Callable[[float, float], float],
                           total_time: float = 3_600.0,
                           dt: float = 1.0,
                           initial_speed: float = 0.0) -> Simulation:
    sim = Simulation()

    # Set Initial State
    vehicle.speed = initial_speed
    vehicle.position = 0.0
    inclination_angle = 0.0
    t_last_disturbance = 0

    for t in np.arange(0, total_time, dt):

        # Check for disturbances
        if int(t) in road_inclinations.keys():
            inclination_angle = road_inclinations[int(t)]
            t_last_disturbance = t
        # Gradual change in the inclination angle after a disturbance to simulate the road leveling
        if t_last_disturbance + 2 > t:
            inclination_angle = adjust_inclination(inclination_angle)

        # [H] - Measure the speed with the Speedometer
        read_speed = measure_element(vehicle.speed)

        # [e] - Calculate the error signal
        error = set_speed - read_speed

        # [Gc] - Apply the controller
        throttle = controller(error, dt)  # %

        # [Gp] - Process the vehicle dynamics
        speed = process(vehicle, throttle, dt, theta=inclination_angle)

        # Store simulation data
        sim.times.append(t)
        sim.speeds.append(speed)
        sim.positions.append(vehicle.position)
        sim.inclinations.append(inclination_angle)
        sim.errors.append(error)

    return sim
InĀ [32]:
def to_km_simulation(sim: Simulation):
    """
    Convert the simulation speeds to km/h.
    
    Args:
        sim: Simulation data
    
    Returns:
        Simulation data with speeds in km/h
    """

    # Convert speeds to km/h
    sim.speeds = [ms_to_kmh(speed) for speed in sim.speeds]
    sim.errors = [ms_to_kmh(error) for error in sim.errors]

    # convert positions to km
    sim.positions = [pos / 1_000 for pos in sim.positions]
InĀ [33]:
def plot_in_kmh(simulation: Simulation, set_speed: float = 0.0):
    """
    Plot the simulation results.
    
    Args:
        simulation: Simulation data
        set_speed: Speed in km/h
    """
    plt.clf()
    plt.close()
    plt.figure(figsize=(20, 15))
    plt.subplot(3, 1, 1)
    plt.plot(simulation.times, simulation.speeds, label='Actual Speed')
    if set_speed > 0.0:
        plt.axhline(y=set_speed + 4, color='greenyellow', linestyle='dotted', label='Speed Upper Bound')
        plt.axhline(y=set_speed, color='darkolivegreen', linestyle='--', label='Step Speed')
        plt.axhline(y=set_speed - 4, color='greenyellow', linestyle='dotted', label='Speed Lower Bound')
    plt.xlabel('Time (s)')
    plt.ylabel('Speed (km/h)')
    plt.title('Vehicle Speed Over Time')
    plt.legend()

    plt.subplot(3, 1, 2)
    plt.plot(simulation.times, simulation.inclinations, label='Īø inclination')
    plt.xlabel('Time (s)')
    plt.ylabel('Inclination (Degrees)')
    plt.title('Road Inclination Over Time')
    plt.legend()

    if len(simulation.errors) > 0:
        plt.subplot(3, 1, 3)
        plt.plot(simulation.times, simulation.errors, label='Error')
        plt.xlabel('Time (s)')
        plt.ylabel('Error (km/h)')
        plt.title('Speed Error Over Time')
        plt.legend()

    plt.tight_layout()
    plt.show()
InĀ [34]:
p_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=p.update, total_time=45 * 60)

to_km_simulation(p_simulation)

plot_in_kmh(p_simulation, set_speed=ms_to_kmh(desired_speed))
No description has been provided for this image
InĀ [35]:
pi_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=pi.update, total_time=45 * 60)

to_km_simulation(pi_simulation)

plot_in_kmh(pi_simulation, set_speed=ms_to_kmh(desired_speed))
No description has been provided for this image
InĀ [36]:
pid_simulation = simulate_with_readings(camry, set_speed=desired_speed, controller=pid.update, total_time=45 * 60)

to_km_simulation(pid_simulation)

plot_in_kmh(pid_simulation, set_speed=ms_to_kmh(desired_speed))
No description has been provided for this image
InĀ [37]:
p_ctrl = PIDController(kp=1)

p_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=p_ctrl.update, total_time=45 * 60)
to_km_simulation(p_opt_sim)

plot_in_kmh(p_opt_sim, set_speed=ms_to_kmh(desired_speed))
No description has been provided for this image
InĀ [38]:
pi_ctrl = PIDController(kp=3, ki=0.005)

pi_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=pi_ctrl.update, total_time=45 * 60)
to_km_simulation(pi_opt_sim)

plot_in_kmh(pi_opt_sim, set_speed=ms_to_kmh(desired_speed))
No description has been provided for this image
InĀ [39]:
pid_ctrl = PIDController(kp=3, ki=0.005, kd=0.4)

pid_opt_sim = simulate_with_readings(camry, set_speed=desired_speed, controller=pid_ctrl.update, total_time=45 * 60)

to_km_simulation(pid_opt_sim)

plot_in_kmh(pid_opt_sim, set_speed=ms_to_kmh(desired_speed))
No description has been provided for this image
InĀ [40]:
data = {
    "Simulation": ["P", "PI", "PID", "P (Optimized)", "PI (Optimized)", "PID (Optimized)"],
    "Max Distance (km)": [max(p_simulation.positions), max(pi_simulation.positions), max(pid_simulation.positions),
                          max(p_opt_sim.positions), max(pi_opt_sim.positions), max(pid_opt_sim.positions)],
    "Average Speed": [sum(p_simulation.speeds) / len(p_simulation.speeds),
                      sum(pi_simulation.speeds) / len(pi_simulation.speeds),
                      sum(pid_simulation.speeds) / len(pid_simulation.speeds),
                      sum(p_opt_sim.speeds) / len(p_opt_sim.speeds),
                      sum(pi_opt_sim.speeds) / len(pi_opt_sim.speeds),
                      sum(pid_opt_sim.speeds) / len(pid_opt_sim.speeds)],
    "Max Speed": [max(p_simulation.speeds), max(pi_simulation.speeds), max(pid_simulation.speeds),
                  max(p_opt_sim.speeds), max(pi_opt_sim.speeds), max(pid_opt_sim.speeds)],
    "Max Error": [max(p_simulation.errors), max(pi_simulation.errors), max(pid_simulation.errors),
                  max(p_opt_sim.errors), max(pi_opt_sim.errors), max(pid_opt_sim.errors)],
    "Min Error": [min(p_simulation.errors), min(pi_simulation.errors), min(pid_simulation.errors),
                  min(p_opt_sim.errors), min(pi_opt_sim.errors), min(pid_opt_sim.errors)],
    "Avg Error": [sum(p_simulation.errors) / len(p_simulation.errors),
                  sum(pi_simulation.errors) / len(pi_simulation.errors),
                  sum(pid_simulation.errors) / len(pid_simulation.errors),
                  sum(p_opt_sim.errors) / len(p_opt_sim.errors),
                  sum(pi_opt_sim.errors) / len(pi_opt_sim.errors), sum(pid_opt_sim.errors) / len(pid_opt_sim.errors)]
}

df_opt = pd.DataFrame(data)
df_opt
Out[40]:
Simulation Max Distance (km) Average Speed Max Speed Max Error Min Error Avg Error
0 P 79.732363 106.349316 114.848624 107.109215 -10.813715 1.662624
1 PI 81.037755 108.092624 186.657745 108.000000 -83.092243 0.002243
2 PID 81.018188 108.062117 188.930768 108.000000 -85.490061 -0.004419
3 P (Optimized) 79.755058 106.379970 114.122368 108.000000 -10.171372 1.654980
4 PI (Optimized) 80.847730 107.838082 116.807322 108.000000 -12.779878 0.291494
5 PID (Optimized) 80.727313 107.676281 116.468743 106.765584 -12.093140 0.379390

Control Analysis¶

Updating the simulation to use the refined package code.

InĀ [41]:
from acc.utils.rv import RoadInclinationGenerator
from acc.model.control import EngineControlUnit
from acc.simulation import SimulationResult, run_simulation
from acc.model import process


def ctrl_simulation(
        kp: float = 0.5,
        ki: float = 0.2,
        kd: float = 1.0,
        windup_protection: bool = False,
        vi: float = 30.0,
        generate_inclinations: bool = False
) -> SimulationResult:
    """
    Run a simulation with a PID controller.
    """
    camry_xse_2025 = process.Vehicle(mass=1_604,
                                     drag_coefficient=0.28,
                                     frontal_area=1.94,
                                     torque_max=221,
                                     omega_max=545.3,
                                     gear_speed_ranges=[(0, 10), (10, 30), (30, 50), (50, 70), (70, 100), (100, 130),
                                                        (130, 160),
                                                        (160, 200)])
    ecu = EngineControlUnit(kp=kp, ki=ki, kd=kd, windup_protection=windup_protection)

    return run_simulation(vehicle=camry_xse_2025,
                          vi=vi,
                          control=ecu,
                          inclination_generator=RoadInclinationGenerator() if generate_inclinations else None,
                          initial_speed=0)

Running the simulation with a PID controller we notice that the speed reaches the upper limit for a long period before approaching the set speed.

InĀ [42]:
from acc.utils import plot

results = ctrl_simulation(kp=2, ki=0.5, kd=4)

plot.plot_results(results, step_speed=30 * 3.6)
No description has been provided for this image
InĀ [43]:
results.df().describe()
Out[43]:
Error Speed Throttle Speedometer
count 3600.000000 3600.000000 3600.000000 3600.000000
mean 0.009106 107.980746 0.976328 107.990894
std 8.232561 7.942230 0.184040 8.232561
min -7.705622 4.303984 -1.000000 0.838604
25% -2.907126 107.556374 1.000000 107.394665
50% -0.933405 108.701037 1.000000 108.933405
75% 0.605335 111.527989 1.000000 110.907126
max 107.161396 111.585574 1.000000 115.705622

Taking a loot at the controller output u(t). It is seen that for the exact same time that the speed reaches the upper limit, the controller output is at its maximum value. While the speed is inside the tolerance, is not quite the desired behavior.

InĀ [44]:
from acc.simulation import SimulationResult


def time_to_steady_state(result: SimulationResult, step_speed=30 * 3.6):
    """
    Calculate the time to reach a steady state.
    
    Args:
        result: Simulation result
        step_speed: Step speed
    
    Returns:
        Time to reach a steady state
    """
    t = -1
    for i, speed in enumerate(result.speeds):
        if speed >= step_speed - 4:
            t = i
            break

    return t
InĀ [45]:
print(f"Time to reach Steady State: {time_to_steady_state(results)} s")
Time to reach Steady State: 136 s

The integral problem¶

The integral control term adds up all the error amounts as time goes by. As long as a difference between $V_i$ and Vo exists, an error will continue accumulating. The faster the value accumulates, the more control output is added to the system since it indicates that we are far from the SP.

If it so happens that in a particular system, we cannot add enough control signal (throttle) to reach the $V_i$, the error will keep accumulating, there’s nothing that can be done.

If this happens, the error keeps building, and the internal calculation of the control system has produced an output of well over 100% of the full output signal. In our vehicle, the ā€˜throttle signal’ is telling you to push the pedal beyond the floor… But this is simply not possible.

The symptom of this effect is seen as a delay between the change in the throttle and the system response. For a while, the control signal is beyond its limit before returning to the range of real-world changes. It is a difficult symptom to diagnose because the reason for the delay is the past error accumulation driving the output too far in the present.

Solution¶

The simplest is to change the Integral term summation if the control output reaches 100%. If this output is reached, we know that the error will still accumulate, it’s unavoidable. We’re driving as fast as we can. In this case, it might be reasonable to halt the accumulation of the Integral term until the SP is changed again

Previous Controller¶

We accumulate the error in the integral term:

def control(error: float):        
    integral += error * dt
    
    derivative = (error - self._previous_error) / dt
    
    # u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt
    output = self.kp * error + self.ki * self._integral + self.kd * derivative
    self._previous_error = error
    
    return np.clip(output, -1, 1)
Windup Protection¶

Adding an optional windup protection to the controller. If the output is maxed out either brake (-100%) or accelerate (100%), stop accumulating error.

def control(error: float, windup: bool):        
    derivative = (error - self._previous_error) / dt
    
    # u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt
    output = self.kp * error + self.ki * self._integral + self.kd * derivative
    self._previous_error = error

    if self.windup_protection:
        # If output is maxed out either brake or accelerate, stop accumulating error
        if not (output == -1 and error < 0) and not (output == 1 and error > 0):
            self._integral += error * dt
    else:
        self._integral += error * dt
    
    return np.clip(output, -1, 1)
InĀ [46]:
results = ctrl_simulation(kp=2, ki=0.5, kd=1.7, windup_protection=True)

plot.plot_results(results, step_speed=30 * 3.6)
No description has been provided for this image

The vehicle reaches the set speed in a shorter time, and the control output is within the limits, as we can see sooner throttle changes.

InĀ [47]:
results.df().describe()
Out[47]:
Error Speed Throttle Speedometer
count 3600.000000 3600.000000 3600.000000 3600.000000
mean 3.019558 104.989875 0.945361 104.980442
std 7.572387 7.306318 0.225944 7.572387
min -2.241880 4.303984 -1.000000 0.701817
25% 1.092793 105.526554 1.000000 105.140010
50% 1.960259 106.117637 1.000000 106.039741
75% 2.859990 106.596135 1.000000 106.907207
max 107.298183 107.871560 1.000000 110.241880

Removing Integral control¶

With the issues caused by the integral term, one can think of removing the component...

InĀ [48]:
results = ctrl_simulation(kp=1, ki=0, kd=0)

plot.plot_results(results, step_speed=30 * 3.6)
No description has been provided for this image

To no surprise, the vehicle never reaches the set speed, as without an accumulation of errors, it is never compensated in the steady state. The e(t) never reaches zero.

InĀ [49]:
results.df().describe()
Out[49]:
Error Speed Throttle Speedometer
count 3600.000000 3600.000000 3600.000000 3600.000000
mean 4.941723 103.082856 0.925511 103.058277
std 7.275457 7.009159 0.136840 7.275457
min 0.608632 4.303984 0.169064 0.000000
25% 3.236269 103.843895 0.898964 103.276018
50% 3.991878 104.075180 1.000000 104.008122
75% 4.723982 104.291125 1.000000 104.763731
max 108.000000 105.031663 1.000000 107.391368